package org.crosswalkproject.xwalkembed;

import java.util.ArrayList;
import java.util.Iterator;
import java.util.List;

import org.opencv.core.Core;
import org.opencv.core.CvType;
import org.opencv.core.Mat;
import org.opencv.core.MatOfPoint;
import org.opencv.core.Point;
import org.opencv.core.Scalar;
import org.opencv.imgproc.Imgproc;
import org.opencv.utils.Converters;

import android.util.Log;

public class ColorBlobDetector {
    // Lower and Upper bounds for range checking in HSV color space
    private Scalar mLowerBound = new Scalar(0);
    private Scalar mUpperBound = new Scalar(0);
    // Minimum contour area in percent for contours filtering
    private static double mMinContourArea = 0.1;
    // Color radius for range checking in HSV color space
    private Scalar mColorRadius = new Scalar(25,50,50,0);
    private Mat mSpectrum = new Mat();
    private List<MatOfPoint> mContours = new ArrayList<MatOfPoint>();
    private static final String  TAG              = "OCVSample::Activity";
    // Cache
    Mat mPyrDownMat = new Mat();
    Mat mHsvMat = new Mat();
    Mat mMask = new Mat();
    Mat mDilatedMask = new Mat();
    Mat mHierarchy = new Mat();
    double  value_x_old=0,value_y_old=0;
	private int phone_pos=0;/*0:;1:;2:;3:;*/
	private int x_old=0;
	private int y_old=0;
	private int area_old=0;
	private int speed_old=0;
	private int area_count=0;

    public void setColorRadius(Scalar radius) {
        mColorRadius = radius;
    }

    public void setHsvColor(Scalar hsvColor) {
        double minH = (hsvColor.val[0] >= mColorRadius.val[0]) ? hsvColor.val[0]-mColorRadius.val[0] : 0;
        double maxH = (hsvColor.val[0]+mColorRadius.val[0] <= 255) ? hsvColor.val[0]+mColorRadius.val[0] : 255;

        mLowerBound.val[0] = minH;
        mUpperBound.val[0] = maxH;

        mLowerBound.val[1] = hsvColor.val[1] - mColorRadius.val[1];
        mUpperBound.val[1] = hsvColor.val[1] + mColorRadius.val[1];

        mLowerBound.val[2] = hsvColor.val[2] - mColorRadius.val[2];
        mUpperBound.val[2] = hsvColor.val[2] + mColorRadius.val[2];

        mLowerBound.val[3] = 0;
        mUpperBound.val[3] = 255;

        Mat spectrumHsv = new Mat(1, (int)(maxH-minH), CvType.CV_8UC3);

        for (int j = 0; j < maxH-minH; j++) {
            byte[] tmp = {(byte)(minH+j), (byte)255, (byte)255};
            spectrumHsv.put(0, j, tmp);
        }

        Imgproc.cvtColor(spectrumHsv, mSpectrum, Imgproc.COLOR_HSV2RGB_FULL, 4);
    }

    public Mat getSpectrum() {
        return mSpectrum;
    }

    public void setMinContourArea(double area) {
        mMinContourArea = area;
    }

    public void set_phone_pos(int mode) {
        phone_pos= mode;
    }

    public void set_robot_pos(int value_x, int x_flag,int value_y,int y_flag,int area) {
    	int p_flag=0;
    	int speed=0;
    	if(value_x!=x_old)
    	{
    		x_old=value_x;
    		speed=5-value_x;
    		if(value_x==0)
    			speed=0;
    		
	        if(x_flag==0)  
	        	CustomModel.getInstance().changecmd("l"+speed+";r-"+speed+";\r\n");
	        else
	        	CustomModel.getInstance().changecmd("r"+speed+";l-"+speed+";\r\n");
	        p_flag=1;
    	}
        /////////
        if(y_flag==1)  
        	value_y=6+value_y;
        else
        	value_y=6-value_y;
        if(value_y!=y_old)
        {
        	y_old=value_y;
        	CustomModel.getInstance().changecmd("x"+(int)value_y+"\r\n");
        	 p_flag=1;
        }
        area_count++;
         
        	
        if(area!=area_old&&area_count%10==0)
        {
        	if(p_flag==0)
        	{
        		speed=Math.abs(area-area_old);
        		if(speed>10000)
        			speed=1;
           		else if(speed>5000)
        			speed=2;
         		else if(speed>2000)
        			speed=3;
         		else if(speed>1000)
        			speed=4;
           		else  
        			speed=0;
        		if(speed_old!=speed)
        		{
		        	if(area_old>area)
		        		CustomModel.getInstance().changecmd("b"+speed+";\r\n");
		        	else
		        		CustomModel.getInstance().changecmd("b-"+speed+";\r\n");
	        	}
        		speed_old=speed;
        	}
        	
        	area_old=area;
        	
        }
    	
    }
  
    
    public void process(Mat rgbaImage) {
        Imgproc.pyrDown(rgbaImage, mPyrDownMat);
        Imgproc.pyrDown(mPyrDownMat, mPyrDownMat);

        Imgproc.cvtColor(mPyrDownMat, mHsvMat, Imgproc.COLOR_RGB2HSV_FULL);

        Core.inRange(mHsvMat, mLowerBound, mUpperBound, mMask);
        Imgproc.dilate(mMask, mDilatedMask, new Mat());

        List<MatOfPoint> contours = new ArrayList<MatOfPoint>();

        Imgproc.findContours(mDilatedMask, contours, mHierarchy, Imgproc.RETR_EXTERNAL, Imgproc.CHAIN_APPROX_SIMPLE);

        // Find max contour area
        double maxArea = 0;
        Iterator<MatOfPoint> each = contours.iterator();
        while (each.hasNext()) {
            MatOfPoint wrapper = each.next();
            double area = Imgproc.contourArea(wrapper);
            if (area > maxArea)
                maxArea = area;
        }

        // Filter contours by area and resize to fit the original image size
        mContours.clear();
        each = contours.iterator();
        while (each.hasNext()) {
            MatOfPoint contour = each.next();
           // if (Imgproc.contourArea(contour) > mMinContourArea*maxArea) {
            if (Imgproc.contourArea(contour) == maxArea&&maxArea>0) {
                Core.multiply(contour, new Scalar(4,4), contour);
                mContours.add(contour);
                List<Point> lista = new ArrayList<Point>();
                Converters.Mat_to_vector_Point(contour, lista);
               // Log.i(TAG, "luojin contour " + lista);
                int i;
                double x=0,y=0;
                for(i=0;i<lista.size();i++)
                {
                   	x+=lista.get(0).x;
                   	y+=lista.get(0).y;
                                   	
                }
                x=x/i;
                y=y/i;
                Log.i(TAG, "luojin contour x:" + x+",y:"+y+",area:"+maxArea+" h:"+rgbaImage.size().height+"w:"+rgbaImage.size().width);
                double value_x=0,value_y=0;
                value_x=rgbaImage.size().width/2-x;
                value_y=rgbaImage.size().height/2-y;
              //  if((value_x_old==value_x)&&(value_y_old==value_y))
                //	break;
                	
              //  value_x_old=value_x;
              //  value_y_old=value_y;
                int x_flag=0,y_flag=0,div=8;
				///////////
                if(value_x<0)
                {
                	value_x=-value_x/(rgbaImage.size().width/div);   
                	x_flag=0;
                	//CustomModel.getInstance().changecmd("l"+(int)value_x+";r-"+(int)value_x+";\r\n");

                }
                else
                {
                	value_x=value_x/(rgbaImage.size().width/div);     
                	x_flag=1;
                	//CustomModel.getInstance().changecmd("r"+(int)value_x+";l-"+(int)value_x+";\r\n");
                }	
                /////////
                ///////////
                if(value_y<0)
                {
                	y_flag=0;
                	value_y=-value_y/(rgbaImage.size().height/div);   
                	//CustomModel.getInstance().changecmd("x"+(int)value_y+"\r\n");
                	
                }
                else
                {
                	y_flag=1;
                	value_y=value_y/(rgbaImage.size().height/div);        
                	//CustomModel.getInstance().changecmd("x"+(int)value_y+"\r\n");
                }	
            	set_robot_pos((int)value_x, x_flag,(int)value_y,y_flag,(int)maxArea);
                /////////
               
            }
        }
    }

    public List<MatOfPoint> getContours() {
        return mContours;
    }
    
}
